/*************************************************************************************************************
*                                                    UTOS
*                               A non-preemptive(cooperative) multitasking RTOS Kernel
*
* Copyright (c) 2013, HuangLiang (huangliang.tj@gmail.com)
* Under New BSD license, see "LICENSE" for detail
*
* @.file       utos_core.c
* @.version    refer to macro "UT_VERSION" in utos.h
* @.brief      
* 
*************************************************************************************************************/
#define    UT_GLOBAL        // Global variables master file
#include   "utos.h"

/* Internal Macro, find high prio
*  if pt_1's prio value is small than pt_2's prio (it means _tcb_1 has higher priority)
*  This macro is 0. Or, it is 1.
*/

#define    UT_TCB_HIGH(pt_1,pt_2)       (((UT_TCB*)(pt_1))->prio > ((UT_TCB*)(pt_2))->prio)
// Unmap table, the same as uCOS-II. index 0 will never be used!
UT_INT8U  const  UT_UnMap_Tbl[256] = {
    0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x00 to 0x0F                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x10 to 0x1F                             */
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x20 to 0x2F                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x30 to 0x3F                             */
    6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x40 to 0x4F                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x50 to 0x5F                             */
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x60 to 0x6F                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x70 to 0x7F                             */
    7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x80 to 0x8F                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0x90 to 0x9F                             */
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0xA0 to 0xAF                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0xB0 to 0xBF                             */
    6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0xC0 to 0xCF                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0xD0 to 0xDF                             */
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,       /* 0xE0 to 0xEF                             */
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0        /* 0xF0 to 0xFF                             */
};
static  void       * UT_Idle_Thread_PQ_Heap[UT_IDLE_PROCESS_PQ_SIZE];

static  void       * UT_Idle_Thread            (void* p_arg);
static  void         UT_Idle_Init              (void);
                                        // sem to trigger idle thread. 
static  void         UT_Core_Reset_HighRdy     (UT_PCB * _p_pcb, UT_TCB * _p_tcb);
static  void         UT_Core_Find_HighRdy_PCB      (void);
void    UT_Init(void)
{
    int i;
    UT_PCB_Rdy_Grp = 0;
    for(i=0;i<UT_PCB_Tbl_Size;i++)
    {
        UT_PCB_Rdy_Tbl[i] = 0;
    }


    UT_Process_Init     ();
    UT_Thread_Init      ();
    UT_Evt_Init         ();
    UT_Sem_Init         ();
    
    // Add Idle process Initial here ! Idle process must always be ready!
    UT_Idle_Init        ();
}

void    UT_Main(void)
{

    UT_Core_Find_HighRdy_PCB();        // find highest ready, set to UT_PCB_Runtime_Cur
    
    if(UT_PCB_Runtime_Cur->stat & UT_PCB_STAT_MUTE)
        return ;                                            // If process muted, return
    
/* Step 2: find highest thread in this process, set to UT_TCB_Runtime_Cur  */
    UT_TCB_Runtime_Cur = (UT_TCB*)(UT_PCB_Runtime_Cur->tpq[1]);
    if(UT_TCB_Runtime_Cur->stat & UT_TCB_STAT_SLEEP)          // If thread asleep, return
    {
        return ;
    }

/* Step 3: Run! except process muted */

    UT_TCB_Runtime_Cur->p_thread_rtn = UT_TCB_Runtime_Cur->
                        thread_callback ( UT_TCB_Runtime_Cur->p_thread_arg ) ;

    
    UT_Core_Reset_HighRdy(UT_PCB_Runtime_Cur, UT_TCB_Runtime_Cur);
}
void    UT_Tick(void)
{
    UT_PCB   *_p_pcb;
    UT_TCB   *_p_tcb;
    _p_pcb = UT_PCB_List;
    for(_p_pcb = UT_PCB_List;  _p_pcb != (UT_PCB*)0; _p_pcb = _p_pcb->p_prev)       // Scan PCBs in linear time
    {
        if(_p_pcb->stat & UT_PCB_STAT_MUTE)
            continue;                                 // If process muted, continue to next PCB iteration
            
        

        for(_p_tcb=_p_pcb->p_tcb_list; _p_tcb!=(UT_TCB*)0; _p_tcb=_p_tcb->p_prev )   // Scan TCBs in linear time
        {
            if(_p_tcb->stat & UT_TCB_STAT_SLEEP)
                continue;                             // If thread sleep, continue to next TCB iteration
            if(_p_tcb->timeout)                       // timeout == 0 means wait forever
            {
                _p_tcb->timeout--;
                if((_p_tcb->timeout == 0) && ((_p_tcb->stat & UT_TCB_STAT_READY) == 0))
                {
                    if(_p_pcb->tpq_entry < _p_pcb->tpq_size)
                    {
                        UT_Core_Set_HighRdy(_p_pcb, _p_tcb);
                    }
                }
            }
        }
    }
}
// Core utility. Must make sure tcb & pcb is legal before call this function
UT_INT8U    UT_Core_Set_HighRdy(UT_PCB *p_proc, UT_TCB   *p_thrd)
{
    /**********      Set ready TCB  ***********/
    int    i;
    void  *p_temp;

    i = p_proc->tpq_entry;
    p_proc->tpq_entry++;
    p_proc->tpq[i]  =   p_thrd;
    //while((i > 1) && (((UT_TCB*)(p_proc->tpq[i/2]))->prio > ((UT_TCB*)(p_proc->tpq[i]))->prio))
    while((i > 1) && UT_TCB_HIGH(p_proc->tpq[i/2], p_proc->tpq[i]))
    {
        p_temp            =  p_proc->tpq[i/2];
        p_proc->tpq[i/2]  =  p_proc->tpq[i];
        p_proc->tpq[i]    =  p_temp;
        
        i=i/2;      // Find mother node
    }
    
    /**********      Set ready PCB  ***********/
    UT_PCB_Rdy_Grp                   |= p_proc->rdy_grp_bit;
    UT_PCB_Rdy_Tbl[p_proc->rdy_grp]  |= p_proc->rdy_tbl_bit;
    
    /**********     Set TCB stat    ***********/
    p_thrd->stat |= UT_TCB_STAT_READY;              // To prevent from push in pq multiple times

    return UT_ERR_NONE;
    /**********     Set PCB stat    ***********/
}
static void    UT_Core_Find_HighRdy_PCB(void)
{
    // In UT_Main(), at least one process is ready -> idle process
    UT_INT8U  high_pcb_prio;
/*  Step 1, find highest prio Process, set to UT_PCB_Runtime_Cur */
#if UT_PROCESS_LOWEST_PRIO <= 63
    {
        UT_INT8U  grp;
        grp           =  UT_UnMap_Tbl[UT_PCB_Rdy_Grp];
        high_pcb_prio =  grp * 8 + UT_UnMap_Tbl[UT_PCB_Rdy_Tbl[grp]];
    }
#else
    {
        UT_INT8U  grp, tbl_offset;
        if(UT_PCB_Rdy_Grp & 0xff)
        {
            grp = UT_UnMap_Tbl[UT_PCB_Rdy_Grp & 0xff];
        }
        else
        {
            grp = 8 + UT_UnMap_Tbl[UT_PCB_Rdy_Grp / 256];     // UT_PCB_Rdy_Grp >> 8
        }
        tbl_offset = UT_PCB_Rdy_Tbl[grp];
        if(tbl_offset & 0xff)
        {
            high_pcb_prio = grp * 16 + UT_UnMap_Tbl[tbl_offset & 0xff];
        }
        else
        {
            high_pcb_prio = grp * 16 + 8 + UT_UnMap_Tbl[tbl_offset / 256];  // tbl_offset >> 8
        }
    }
#endif
    UT_PCB_Runtime_Cur = UT_PCB_Prio_Tbl[high_pcb_prio];        // Set UT_PCB_Runtime_Cur to current highest
}
static  void   UT_Core_Reset_HighRdy(UT_PCB * _p_pcb, UT_TCB * _p_tcb)
{
    UT_INT8U i , j;
    void * p_temp;
    /* Step 4: Cost and re-initial  */

    if(_p_tcb->stat & UT_TCB_STAT_EVT_PEND)
    {
        if(_p_tcb->p_event->event_opt == UT_EVT_OPT_NOT_COST)
            return ;                                              // Event may cause not reinit and return!
        // call flags and other event to reinit event it self. 
        // If a thread has both a none-zero timeout and an event set to not-cost, the timeout will count down to zero
        // ... and never be reinitialed, can never be triggered by UT_Tick() 
        
    }
        /**** Re-set timeout ****/
    _p_tcb->timeout  = _p_tcb->timeout_reg;
        /* Reset TCB stat */
    _p_tcb->stat    &= ~UT_TCB_STAT_READY;
                    // This state is not used to determine ready, just prevent from push into pq multipule times
    //if(UT_PCB_Runtime_Cur->tpq_entry > 1)
    /**********     for TCB     ***********/
    _p_pcb->tpq_entry--;
    _p_pcb->tpq[1] = _p_pcb->tpq[_p_pcb->tpq_entry];
    i = 1;
    while( 2 * i < _p_pcb->tpq_entry )
    {
        j = 2 * i;      // Left child
        /* First find which child is bigger, in order to exchange it with parent */
        //if( (j < (_p_pcb->tpq_entry-1)) && ( (( UT_TCB *)(_p_pcb->tpq[j]))->prio > (( UT_TCB *)(_p_pcb->tpq[j+1]))->prio))
        if((j < (_p_pcb->tpq_entry-1)) &&  UT_TCB_HIGH(_p_pcb->tpq[j], _p_pcb->tpq[j+1]))
        {
            j += 1;
        }
        /* Next, compare child with parent, if parent is smaller, then exchange them.
           if not, then the tree is legitimate, we can break now .
        */
        if(!UT_TCB_HIGH(_p_pcb->tpq[i],_p_pcb->tpq[j]))
        {
            break;
        }
        p_temp            = _p_pcb->tpq[i];
        _p_pcb->tpq[i] = _p_pcb->tpq[j];
        _p_pcb->tpq[j] = p_temp;
        
        i  = j;
    }
    
    /**********     for PCB     ***********/
    if(_p_pcb->tpq_entry <= 1)
    {
        UT_PCB_Rdy_Tbl[_p_pcb->rdy_grp]  &= ~(_p_pcb->rdy_tbl_bit);
        if( UT_PCB_Rdy_Tbl[_p_pcb->rdy_grp] == 0)
            UT_PCB_Rdy_Grp               &= ~(_p_pcb->rdy_grp_bit);
    }
}
static void   UT_Idle_Init(void)
{
    UT_SEM   * UT_Idle_SEM;   //This internal sem will be killed after this function call
    UT_Process_Create (UT_IDLE_PROCESS_PRIO, &UT_Idle_Thread_PQ_Heap[0], UT_IDLE_PROCESS_PQ_SIZE);
    UT_Thread_Create  (UT_IDLE_PROCESS_PRIO, UT_IDLE_THREAD_PRIO, UT_Idle_Thread, 0, (void*)0);
    UT_Idle_SEM = UT_Sem_Create_Prio(UT_IDLE_PROCESS_PRIO, UT_IDLE_THREAD_PRIO);
            // We will never cost this sem , let idle thread always be ready.
    UT_Evt_Set_Opt((UT_EVENT*)UT_Idle_SEM->ptr, UT_EVT_OPT_NOT_COST);
    UT_Thread_Arg_Set(UT_IDLE_PROCESS_PRIO, UT_IDLE_THREAD_PRIO, UT_Idle_SEM);
            // We can store UT_Idle_SEM in idle thread's arg. And no need to modify this sem
    UT_Sem_Post(UT_Idle_SEM);
}
static void * UT_Idle_Thread(void* p_arg)
{
    p_arg = p_arg;      // p_arg is actually UT_Idle_SEM, must not modify it! or system will crush
}
/*************************************************************************************************************
*                                                  END
*************************************************************************************************************/
